#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <cmath>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Image.h>
//#include <laser_test/aimPose.h>
#include <laser_test/laser_control.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include "opencv2/opencv.hpp"
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h> 
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl_ros/point_cloud.h>
using namespace std;
using namespace cv;
#define dianji_fuhao -1
#define integralMax 10
#define integralMin -10
float nearest_x,nearest_y,nearest_dist;
ackermann_msgs::AckermannDriveStamped  ackermann_cmd;
sensor_msgs::LaserScan  zeal_laser; // 雷达数据的副本，用于后续函数值传递，可以优化成常引用传递
std_msgs::Int32 call_back_data;
float k = 0;
float nearest_angle;
                                                                                                                                                                                                               
float midx,midy,sx=0,sy=0;
laser_test::laser_control laser_control;
laser_geometry::LaserProjection projector_;
sensor_msgs::PointCloud2 cloud;
int num = 1440;//num为雷达扫描一周的点数
//int low_range = num/16,up_range = num*7/16;
int low_range = 180,up_range = 1440,middle_range=1260;
//int low_range = 1,up_range2 = 120;

/***********避障参数 */
int flag = 0;
int distance_aheadflag = 0;
int park_cmdflag = 0;
/***********避障参数 */

/**********倒车入库参数 */
int daoche_ruku_flag = 1;
int daoche_ruku_dist_index = 1000;
int daoche_ruku_angle = 10000;
int copy_range_shuzu[360];
/**********倒车入库参数 */

/**********正车入库参数 */
int zhengche_ruku_flag = 1;
int zhengche_ruku_dist_index = 1000;
int zhengche_ruku_angle = 10000;
/**********正车入库参数 */

/**********侧方停车参数 */
int cefang_tingche_flag = 1;
int cefang_tingche_dist_index = 1000;
int saomiao_open_flag = 1;
int jiaodu_jiaozhuen_count = 0;
int right_angle_flag = 2;
int zeal_count = 0;
float jiaodu;
int jiaodu_jiaodu = 0;
int my_final_juli = 0;
float my_final_jiaodu = 0;
/**********侧方停车参数 */

/**********PID参数 */
float lastlastError = 0.0;
float lastError = 0.0;
float integral = 0.0;

/**********PID参数 */
/**********自定义函数 */
int zeal_get_nearest_dist_index(float my_nearest_angle); // 获取弧度角度的距离索引
void right_daocheruku_pingyi();
void left_daocheruku_pingyi();
float zeal_get_my_nearest_angle(int zuo_bianjie, int you_bianjie);
float zeal_get_my_nearest_angle_julixianzhi(int zuo_bianjie, int you_bianjie,float juli_xianzhi);//侧方停车
int zeal_back_mid_error_jiaozhun(float my_nearest_angle,float speed);
int zeal_front_mid_error_jiaozhun(float my_nearest_angle,float speed);
float zeal_pid_contoler(float angle_now,float setpoint,float kp,float ki,float kd);
/**********自定义函数 */
ros::Publisher pub_vel;
ros::Publisher pub_call_back;
ros::Duration duration1(3);
ros::Duration duration2(3);
ros::Duration duration3(3);
ros::Duration duration4(3);
ros::Duration duration5(3);
ros::Duration duration6(3);
ros::Duration duration7(3);

// float abs(float x)
// {
//   if(x<0) x = -x;

//   else if(x>=0) x =  x;

//   return x;
// }

void Park_callback(const std_msgs::Int32::ConstPtr& msg) // 接收主节点发过来的命令
{
  park_cmdflag = msg->data;
}



void LaserOP(const sensor_msgs::LaserScan::ConstPtr& laser)
{

zeal_laser = *laser; // 将接收到的雷达数据赋值给zeal_laser,方便自定义的函数使用

  //ackermann_cmd.drive.speed = ackermann_cmd.drive.speed;
  tf::TransformListener tfListener_;
  projector_.transformLaserScanToPointCloud("laser_link", zeal_laser, cloud, tfListener_);
  int i,tt=0;
  int l=100,r=100;
  float xba=0,yba=0,px1,py1,px2,py2,similarity=-1,d1=0,d2=0;
  nearest_x=0; nearest_y=100; nearest_dist=100;

  if (park_cmdflag==9)//////////////////9为开始识别避障
  {
        for(i=0;i<1440;i=i+4)
    {
    float ang=zeal_laser.angle_min+i*zeal_laser.angle_increment;
    float px=-sin(ang)*zeal_laser.ranges[i];
    float py= cos(ang)*zeal_laser.ranges[i];

    if (zeal_laser.ranges[i]<nearest_dist )
    {
      tt++;
      if (tt==3)
      {
        nearest_dist=zeal_laser.ranges[i]; nearest_x=px; nearest_y=py; tt=0; l=i; r=i;
        nearest_angle = zeal_laser.angle_min+i*zeal_laser.angle_increment;

      }
    }
    else
      tt=0;
    }
  tt=0;
  ROS_INFO_STREAM("The nearest distance is: "<<nearest_dist<<" The nearest angle is: "<<nearest_angle/CV_PI*180<<endl);

  /* ******************/
  if(nearest_angle/CV_PI*180+90>90&&nearest_angle/CV_PI*180<112.5)
  {
    distance_aheadflag = 1;
  }
  else if (nearest_angle/CV_PI*180>67.5&nearest_angle/CV_PI*180<90)
  {
    distance_aheadflag = 1;
  }
  else
  {
    distance_aheadflag = 0;
  }
  /* *******************/
  if((nearest_dist<0.451)&&(distance_aheadflag==1))
  {
    ROS_INFO_STREAM("The obstacle is detected!!!!!!!!!"<<endl);
    call_back_data.data = 1; // 为1代表发现障碍，停车并向主节点请求避障
    pub_call_back.publish(call_back_data);
  }
  }
  /* *******************/

  else if (park_cmdflag==10 ) //10为开始执行避障
  {
   //这里是避障的动作组，分为5个动作，后退拉开，左转调整，斜线直走，右转调正，斜线直走回到巡线，共5个动作
    // ROS_INFO_STREAM("The obstacle is detected!aaaaaaaaaaaaaa"<<endl);
    laser_control.laser_control = 1;
    // ackermann_cmd.drive.speed = -20*dianji_fuhao;

    if(flag == 0 )
    { //1
        ROS_INFO_STREAM("turn left"<<endl);
        ackermann_cmd.drive.steering_angle = -70;//左转弯  角度需要调参
        ackermann_cmd.drive.speed = -20*dianji_fuhao;
        pub_vel.publish(ackermann_cmd);
        duration1 = ros::Duration(4);//延迟4秒
        duration1.sleep();//调用延迟函数
        flag = 1;

    }

    if (flag == 1)
    {
        ROS_INFO_STREAM("straight1111"<<endl);
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = -25*dianji_fuhao;
        pub_vel.publish(ackermann_cmd);
        duration2 = ros::Duration(1.5);//延迟3秒
        duration2.sleep();//调用延迟函数
        flag = 2;

    }

    if(flag == 2)
    {
        ROS_INFO_STREAM("turn right"<<endl);
        ackermann_cmd.drive.steering_angle = 70;//右转弯  角度需要调参
        ackermann_cmd.drive.speed = -20*dianji_fuhao;
        pub_vel.publish(ackermann_cmd);
        duration3 = ros::Duration(6);//延迟3秒
        duration3.sleep();//调用延迟函数
        flag = 3;
    }

    if(flag == 3)
    {
        ROS_INFO_STREAM("straight2222"<<endl);
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = -25*dianji_fuhao;
        pub_vel.publish(ackermann_cmd);
        duration4 = ros::Duration(1.5);//延迟3秒
        duration4.sleep();//调用延迟函数
        flag = 4;
    }

    if(flag == 4)
    {
        ROS_INFO_STREAM("stop"<<endl);
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = 0;
        pub_vel.publish(ackermann_cmd);
        call_back_data.data = 2; // 为2代表避障结束，向主节点发送信息
        pub_call_back.publish(call_back_data);
        flag = 5; //标志位，代表避障结束
    }

  }
  else if (park_cmdflag == 11) //11为开始执行倒车入库
  {
    /*
    int zeal_get_my_nearest_angle(int zuo_bianjie, int you_bianjie, sensor_msgs::LaserScan  my_laser);
    int zeal_back_mid_error_jiaozhun(int my_nearest_angle)
    */
      float jiaodu = zeal_get_my_nearest_angle(250,290);
      int jiaodu_jiaodu = jiaodu/CV_PI*180;
    if(daoche_ruku_flag == 1)
    {
      // float jiaodu = zeal_get_my_nearest_angle(225, 315);
      // int jiaodu_jiaodu = jiaodu/CV_PI*180;
      ROS_INFO_STREAM("shuju"<<jiaodu_jiaodu<<endl);
      daoche_ruku_dist_index = zeal_back_mid_error_jiaozhun(jiaodu,20);
      if(zeal_laser.ranges[daoche_ruku_dist_index] < 1.2)
      {
        daoche_ruku_flag =2;
      }
    }

    else if(daoche_ruku_flag == 2)
    {
        // ackermann_cmd.drive.steering_angle = -3;
        // ackermann_cmd.drive.speed = 25*dianji_fuhao;
        // pub_vel.publish(ackermann_cmd);
        ROS_INFO_STREAM("1M_shuju"<<jiaodu_jiaodu<<endl);
        if(zeal_laser.ranges[zeal_back_mid_error_jiaozhun(jiaodu,15)] < 0.43 )
        {
          ackermann_cmd.drive.steering_angle = 0;
          ackermann_cmd.drive.speed = 0*dianji_fuhao;
          pub_vel.publish(ackermann_cmd);
          daoche_ruku_flag += 1;
        }
    }
    else if(daoche_ruku_flag == 3)
    {
        call_back_data.data = 3; // 为3代表倒车入库结束，向主节点发送信息
        pub_call_back.publish(call_back_data);
        daoche_ruku_flag += 1;
    }
  }



  else if (park_cmdflag == 12) //12为开始执行正车入库
  {
      float jiaodu = zeal_get_my_nearest_angle(60, 120);
      int jiaodu_jiaodu = jiaodu/CV_PI*180;
      if(zhengche_ruku_flag == 1)
    {
      // float jiaodu = zeal_get_my_nearest_angle(225, 315);
      // int jiaodu_jiaodu = jiaodu/CV_PI*180;
      ROS_INFO_STREAM("shuju"<<jiaodu_jiaodu<<endl);
      zhengche_ruku_dist_index = zeal_front_mid_error_jiaozhun(jiaodu,-20);
      if(zeal_laser.ranges[zhengche_ruku_dist_index] < 1.2)
      {
        zhengche_ruku_flag =2;
      }
    }

    else if(zhengche_ruku_flag == 2)
    {
        // ackermann_cmd.drive.steering_angle = -3;
        // ackermann_cmd.drive.speed = 25*dianji_fuhao;
        // pub_vel.publish(ackermann_cmd);
        ROS_INFO_STREAM("1M_shuju"<<jiaodu_jiaodu<<endl);
        ROS_INFO_STREAM("zhengcheruku_distance: "<< zeal_laser.ranges[zeal_front_mid_error_jiaozhun(jiaodu,-15)] << endl);
        if(zeal_laser.ranges[zeal_front_mid_error_jiaozhun(jiaodu,-15)] < 0.45 )
        {
          ackermann_cmd.drive.steering_angle = 0;
          ackermann_cmd.drive.speed = 0*dianji_fuhao;
          pub_vel.publish(ackermann_cmd);
          zhengche_ruku_flag += 1;
        }
    }
    else if(zhengche_ruku_flag == 3)
    {
        call_back_data.data = 4; // 为4代表正车入库结束，向主节点发送信息
        pub_call_back.publish(call_back_data);
        zhengche_ruku_flag += 1;
    }
  }

  else if (park_cmdflag == 13) //13为开始识别侧方停车牌子
  {
      float jiaodu = zeal_get_my_nearest_angle(35,90);
      int jiaodu_jiaodu = jiaodu/CV_PI*180;
      ROS_INFO_STREAM("angle0: "<<jiaodu_jiaodu<<endl);
      cefang_tingche_dist_index = zeal_get_nearest_dist_index(jiaodu);//进入侧方停车后，向前识别牌子的速度为20
      ROS_INFO_STREAM("distance0: "<<zeal_laser.ranges[cefang_tingche_dist_index]<<endl);
    
      if(zeal_laser.ranges[cefang_tingche_dist_index] < 0.7)
      {
        ROS_INFO_STREAM("start_cefang_tingche!!!!!!!!!"<<endl);
        ROS_INFO_STREAM(jiaodu_jiaodu<<endl);
        call_back_data.data = 5; // 为1代表发现障碍，停车并向主节点请求避障
        pub_call_back.publish(call_back_data);
      }
  }

  else if (park_cmdflag == 14) //14为开始执行侧方停车,向主节点发送6代表结束侧方停车
  {
    my_final_jiaodu = zeal_get_my_nearest_angle(70,100);
    float final_dist_index = zeal_get_nearest_dist_index(my_final_jiaodu);
    float distance = zeal_laser.ranges[final_dist_index];

    //全局打印侧方停车flag
    std:: cout << "cefang1_flag:" << cefang_tingche_flag << endl;
    //全局打印角度和距离
    ROS_INFO_STREAM("angle: " << my_final_jiaodu/CV_PI*180 << endl);
    ROS_INFO_STREAM("distance: " << distance << endl);
    if(cefang_tingche_flag == 1)// 1阶段粗调，大致转向，使车身大致进入侧方停车区，同时使雷达可以时刻捕获牌子
    {
        //动作组1：左前
        ackermann_cmd.drive.steering_angle = -50;
        ackermann_cmd.drive.speed = -20*dianji_fuhao;
        pub_vel.publish(ackermann_cmd);
        duration7 = ros::Duration(4);//延迟4秒
        duration7.sleep();//调用延迟函数

        //动作组2：右后
        ackermann_cmd.drive.steering_angle = 50;
        ackermann_cmd.drive.speed = 20*dianji_fuhao;
        pub_vel.publish(ackermann_cmd);
        duration7 = ros::Duration(2);//延迟2秒
        duration7.sleep();//调用延迟函数

        //-----------------------------------------------------------
        //调试延迟模块1，可删除
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = 0*dianji_fuhao;                 
        pub_vel.publish(ackermann_cmd);
        duration7 = ros::Duration(5);//延迟1.5秒
        duration7.sleep();//调用延迟函数
        //-----------------------------------------------------------

        //动作组3：后退
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = 20*dianji_fuhao;
        pub_vel.publish(ackermann_cmd);
        duration7 = ros::Duration(7);//延迟5秒
        duration7.sleep();//调用延迟函数

        //-----------------------------------------------------------------
        //调试延迟模块2，可删除
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = 0*dianji_fuhao;                 
        pub_vel.publish(ackermann_cmd);
        duration7 = ros::Duration(5);//延迟1.5秒
        duration7.sleep();//调用延迟函数
        //-----------------------------------------------------------------

        // 停止，进入二阶段
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = 0*dianji_fuhao;                 
        pub_vel.publish(ackermann_cmd);
        cefang_tingche_flag =2 ;
    }

    else if(cefang_tingche_flag == 2)// 2阶段粗调，直直后退，使车身进入侧方停车区
    {
      
      if(saomiao_open_flag == 1)
      {
            std::cout << "Enter_saomiao_process" << std::endl;
            jiaodu = zeal_get_my_nearest_angle(0,110);
            jiaodu_jiaodu = jiaodu/CV_PI*180;
            int jiaodu_xiuzheng_90_error_ = 90-jiaodu_jiaodu;
            //进行绝对值判断
            int jiaodu_xiuzheng_abs = abs(jiaodu_xiuzheng_90_error_);
            if (jiaodu_xiuzheng_abs <= 20)
            {
                std::cout << "enter_next_cefang_flag________________!!!!" << std::endl;
                cefang_tingche_flag = 6;
            }
            jiaodu_jiaozhuen_count = jiaodu_xiuzheng_90_error_/20;
            ROS_INFO_STREAM("xiuzhengjiaodu  "<< jiaodu_xiuzheng_90_error_ <<endl);
            ROS_INFO_STREAM("cishu "<<jiaodu_jiaozhuen_count <<endl);

            //扫描结束，标志清零
            saomiao_open_flag = 0;
      }

      else if (saomiao_open_flag == 0)
      {
        std::cout << "enter_excute_process" << std::endl;
        if(jiaodu_jiaozhuen_count > 0) right_angle_flag = 1;
        else if(jiaodu_jiaozhuen_count < 0) right_angle_flag = 0;

        //出错的情况，直接报错
        else if(jiaodu_jiaozhuen_count == 0) 
        {
          right_angle_flag = -1;
          std::cout << "cefang_count is zero" << std::endl;
        }


      //校正次数取绝对值
      jiaodu_jiaozhuen_count = abs(jiaodu_jiaozhuen_count);

        while(jiaodu_jiaozhuen_count)
        {
          jiaodu_jiaozhuen_count--;
          if(right_angle_flag == 1)
          {
            ackermann_cmd.drive.steering_angle = 50;
            ackermann_cmd.drive.speed = -15*dianji_fuhao;                 
            pub_vel.publish(ackermann_cmd);
            duration7 = ros::Duration(1);//延迟3秒
            duration7.sleep();//调用延迟函数


            ackermann_cmd.drive.steering_angle = -50;
            ackermann_cmd.drive.speed = 15*dianji_fuhao;                 
            pub_vel.publish(ackermann_cmd);
            duration7 = ros::Duration(1);//延迟3秒
            duration7.sleep();//调用延迟函数
          }
          else if(right_angle_flag == 0)
          {
            ackermann_cmd.drive.steering_angle = -50;
            ackermann_cmd.drive.speed = -15*dianji_fuhao;                 
            pub_vel.publish(ackermann_cmd);
            duration7 = ros::Duration(1);//延迟3秒
            duration7.sleep();//调用延迟函数


            ackermann_cmd.drive.steering_angle = 50;
            ackermann_cmd.drive.speed = 15*dianji_fuhao;                 
            pub_vel.publish(ackermann_cmd);
            duration7 = ros::Duration(1);//延迟3秒
            duration7.sleep();//调用延迟函数
          }
          
        }
        // 重新扫描
        saomiao_open_flag = 1;
      }
    }





    else if(cefang_tingche_flag == 4)// 4阶段细调，只需要套正车入库逻辑即可，目的是对准牌子，然后停下，再调整距离
    {
        //停车
        ackermann_cmd.drive.steering_angle = 0;
        ackermann_cmd.drive.speed = 0*dianji_fuhao;                 
        pub_vel.publish(ackermann_cmd);

    }

    else if(cefang_tingche_flag == 5)// 5阶段细调，调整距离，来停到对应的车库中
    {
    //   duration7 = ros::Duration(2);//延迟3秒
    //   duration7.sleep();//调用延迟函数
      jiaodu = zeal_get_my_nearest_angle(60,110);
      jiaodu_jiaodu = jiaodu/CV_PI*180;
      int dist_index = 4*(int)(jiaodu/CV_PI*180);
      int dist = zeal_laser.ranges[dist_index];
      std::cout << "distance:"<<dist<<std::endl;
      if(dist >= 0 && dist < 10)
      {
        zeal_count++;
        std::cout<<"youxiao_shuju"<<dist<<std::endl;
      }
      else zeal_count = 0;
      if(zeal_count > 3)
      {
           
           if(zeal_laser.ranges[zeal_front_mid_error_jiaozhun(jiaodu,20)] > 1.2 )//这里给出距离要求
        {   
          ackermann_cmd.drive.steering_angle = 0;
          ackermann_cmd.drive.speed = 0*dianji_fuhao;
          pub_vel.publish(ackermann_cmd);
          cefang_tingche_flag = 6;
        } 

      }
      
    }

    else if(cefang_tingche_flag == 6)// 6阶段结束侧方停车，向主节点发送6代表结束侧方停车
    {
      std::cout << "enter_six_cefang_flag________________6666" << std::endl;
     // 告诉主节点，侧方停车结束
      call_back_data.data = 6;
      pub_call_back.publish(call_back_data);
    //进入下一个停车阶段
      cefang_tingche_flag += 1;

    //停下来
      ackermann_cmd.drive.steering_angle = 0;
      ackermann_cmd.drive.speed = 0*dianji_fuhao;
      pub_vel.publish(ackermann_cmd);
          
    }
  }
}//////回调函数结束
//每次调用平移结束后需要进行一段前进操作
void right_daocheruku_pingyi()
{
  ackermann_cmd.drive.steering_angle = 50;
  ackermann_cmd.drive.speed = 20*dianji_fuhao;
  pub_vel.publish(ackermann_cmd);
  // duration6 = ros::Duration(abs(thta - 180)*0.1);//
  duration6 = ros::Duration(1.5);//
  duration6.sleep();//调用延迟函数
  ackermann_cmd.drive.steering_angle = -50;
  ackermann_cmd.drive.speed = 20*dianji_fuhao;
  pub_vel.publish(ackermann_cmd);
  // duration6 = ros::Duration(abs(thta - 180)*0.1);//
  duration6 = ros::Duration(1.5);//
  duration6.sleep();//调用延迟函数
  ackermann_cmd.drive.steering_angle = -3;
  ackermann_cmd.drive.speed = -20*dianji_fuhao;
  pub_vel.publish(ackermann_cmd);
  // duration6 = ros::Duration(abs(thta - 180)*0.1);//
  duration6 = ros::Duration(3*0.9);//
  duration6.sleep();//调用延迟函数
}

void left_daocheruku_pingyi()
{
  ackermann_cmd.drive.steering_angle = -50;
  ackermann_cmd.drive.speed = 20*dianji_fuhao;
  pub_vel.publish(ackermann_cmd);
  // duration6 = ros::Duration(abs(thta - 180)*0.1);//
  duration6 = ros::Duration(1.5);//
  duration6.sleep();//调用延迟函数
  ackermann_cmd.drive.steering_angle = 50;
  ackermann_cmd.drive.speed = 20*dianji_fuhao;
  pub_vel.publish(ackermann_cmd);
  // duration6 = ros::Duration(abs(thta - 180)*0.1);//
  duration6 = ros::Duration(1.5);//
  duration6.sleep();//调用延迟函数
  ackermann_cmd.drive.steering_angle = -3;
  ackermann_cmd.drive.speed = -20*dianji_fuhao;
  pub_vel.publish(ackermann_cmd);
  // duration6 = ros::Duration(abs(thta - 180)*0.1);//
  duration6 = ros::Duration(3*0.9);//
  duration6.sleep();//调用延迟函数
}
/**
 * 函数名：zeal_get_my_nearest_angle
 * @breif:获取指定角度范围内的最近障碍物距离和角度
 * @param zuo_bianjie 左边界，角度
 * @param you_bianjie 右边界，角度
 * @param my_laser 激光雷达数据
 * @return 最近障碍物角度，弧度
 */
float zeal_get_my_nearest_angle(int zuo_bianjie, int you_bianjie)//左右边界单位是°
{
 int l = 4*zuo_bianjie;
 int r = 4*you_bianjie;
 int tt=0;
  float my_nearest_angle=0;
  float my_nearest_dist=100;
 for(int i=l;i<r;i=i+4)
    {
    if (zeal_laser.ranges[i]<my_nearest_dist)
    {
      tt++;
      if (tt==1)
      {
        my_nearest_dist=zeal_laser.ranges[i];
        my_nearest_angle =(zeal_laser.angle_min+i*zeal_laser.angle_increment);
      }
    }
    else
      tt=0;
    }

return my_nearest_angle;
}
//此函数用在侧方停车，加了距离限制，距离太近的物体会过滤掉
float zeal_get_my_nearest_angle_julixianzhi(int zuo_bianjie, int you_bianjie,float juli_xianzhi)//左右边界单位是°
{
 int l = 4*zuo_bianjie;
 int r = 4*you_bianjie;
 int tt=0;
  float my_nearest_angle=0;
  float my_nearest_dist=100;
 for(int i=l;i<r;i=i+4)
    {
    if (zeal_laser.ranges[i]<my_nearest_dist && zeal_laser.ranges[i]>juli_xianzhi)
    {
      tt++;
      if (tt==1)
      {
        my_nearest_dist=zeal_laser.ranges[i];
        my_nearest_angle =(zeal_laser.angle_min+i*zeal_laser.angle_increment);
      }
    }
    else
      tt=0;
    }

return my_nearest_angle;
}
/**
 * 函数名：zeal_back_mid_error_jiaozhun
 * @breif:根据获得的角度进行倒车校正
 * @param my_nearest_angle 最近障碍物角度，弧度，值一般来自zeal_get_my_nearest_angle函数
 * @param speed 速度,在函数内部已做符号修正，目前是负数前进，正数后退
 * @return 最近障碍物距离的索引值
 */
int zeal_back_mid_error_jiaozhun(float my_nearest_angle,float speed)//这里认为期望角度为270°,返回值是当前最近角度对应距离的索引值
{
  int dist_index = 4*(int)(my_nearest_angle/CV_PI*180);
  // float mid_error = 270 - my_nearest_angle/CV_PI*180;
  static float back_integral_angle = 0;
    // ackermann_cmd.drive.steering_angle = -0.5*mid_error;
    back_integral_angle = zeal_pid_contoler(my_nearest_angle/CV_PI*180,270.0,0,2.5,0);//这里为只用p调试结果
    ackermann_cmd.drive.steering_angle = back_integral_angle;
    ackermann_cmd.drive.speed = speed*dianji_fuhao;
    pub_vel.publish(ackermann_cmd);

    dist_index = dist_index < 0 ? 0 : dist_index;
    dist_index = dist_index > 1436 ? 1436 : dist_index;

  return dist_index;

}
/**
 * 函数名：zeal_get_nearest_dist_index
 * @param my_nearest_angle 最近障碍物角度，弧度，值一般来自zeal_get_my_nearest_angle函数
 * @return 输入角度对应距离的索引值
 */
int zeal_get_nearest_dist_index(float my_nearest_angle)
{
  int dist_index = 4*(int)(my_nearest_angle/CV_PI*180);
  dist_index = dist_index < 0 ? 0 : dist_index;
  dist_index = dist_index > 1436 ? 1436 : dist_index;
  return dist_index;
}
/**
 * 函数名：zeal_front_mid_error_jiaozhun
 * @breif:根据获得的角度进行前进校正
 * @param my_nearest_angle 最近障碍物角度，弧度，值一般来自zeal_get_my_nearest_angle函数
 * @param speed 速度,在函数内部已做符号修正，目前是负数前进，正数后退
 * @return 最近障碍物距离的索引值
 */
int zeal_front_mid_error_jiaozhun(float my_nearest_angle,float speed)//这里认为期望角度为90°,返回值是当前最近角度对应距离的索引值
{
int dist_index = 4*(int)(my_nearest_angle/CV_PI*180);
  // float mid_error = 270 - my_nearest_angle/CV_PI*180;
  static float front_integral_angle = 0;
    // ackermann_cmd.drive.steering_angle = -0.5*mid_error;
    front_integral_angle = -zeal_pid_contoler(my_nearest_angle/CV_PI*180,80.0,0,2.5,0);//这里为只用p调试结果
    ackermann_cmd.drive.steering_angle = front_integral_angle;
    ackermann_cmd.drive.speed = speed*dianji_fuhao;
    pub_vel.publish(ackermann_cmd);

    dist_index = dist_index < 0 ? 0 : dist_index;
    dist_index = dist_index > 1436 ? 1436 : dist_index;

  return dist_index;

}

/**
 * 函数名：zeal_get_my_nearest_dist
 * pid控制器
 * @breif:根据pid控制器进行角度位置控制
 * @param angle_now 当前角度，角度（输入进来时需要预处理）
 * @param setpoint 目标角度，角度
 * @param kp p系数
 * @param ki i系数
 * @param kd d系数
 * @return 控制量，角度输出量
 * 增量式pid
 */
float zeal_pid_contoler(float angle_now,float setpoint,float kp,float ki,float kd)
{
    float error = angle_now - setpoint;
    float derivative = (error - lastError) ;
    float derivative_last = error - 2*lastError + lastlastError;
    float output = kp * derivative + ki * error + kd * derivative_last;
    lastError = error;
    lastlastError = lastError;
    //对输出信号限幅
    // if (output > 10) {
    //     output = 10;
    // }
    // else if (output < -10) {
    //     output = -10;
    // }
    return output;

}


// int zeal_Partition(int r[],int low,int high)
// {
//   int i = low,j = high,pivot = r[low];//确定基准元素
//   while(i<j)
//   {
//     while(i<j&&r[j]>pivot)
//     {j--;}     //向左扫描
//     if(i<j)
//     { swap(r[i++],r[j]);}      //在r[i]和r[j]交换后，i+1,右移一位
//     while(i<j&&r[i]<=pivot)
//     { i++; }   //向右扫描
//     if(i<j)
//     {swap(r[i],r[j--]);}      //在r[i]和r[j]交换后，j-1,左移一位

//   }
//   return i ; // 返回递归的基准位置，用于后续分治后快排

// }

// void zeal_QuickSort(int r[],int low,int high)
// {
//   if(low < high)
//   {
//     int mid = zeal_Partition(r,low,high); // 划分
//     zeal_QuickSort(r,low,mid-1); // 左区间递归快排
//     zeal_QuickSort(r,mid+1,high); //右区间递归快排
//   }
// }

int main(int argc,char** argv){
  ros::init(argc,argv,"laser_test");
  ros::NodeHandle nh;

  ros::Subscriber sublaser=nh.subscribe("/scan",1,&LaserOP);
  ros::Subscriber subpark=nh.subscribe("/park_cmd",1,&Park_callback);//由主节点发送过来
  //park_cmdflag=9时，为识别避障，10为开始执行避障；park_cmdflag=2时，为正车入库；park_cmdflag=3时，为侧方停车
  pub_vel=nh.advertise<ackermann_msgs::AckermannDriveStamped>("/ackermann_cmd",1);
  ros::Publisher pub_laser_control=nh.advertise<laser_test::laser_control>("/laser_control",1);
  ros::Publisher pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("/PointCloud", 1);
  pub_call_back = nh.advertise<std_msgs::Int32>("/bizhang_callback", 1);
  ros::Rate rate(40);
  while (ros::ok())
  {
    ros::spinOnce();

    pub_cloud.publish(cloud);
    pub_laser_control.publish(laser_control);
    //ROS_INFO_STREAM("laser_control is: "<<laser_control.laser_control<<endl);
    rate.sleep();
  }

}


